#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "key.h"
#include "can.h"
#include "encoder.h"
#include "wb_tick.h"
#include "wb_timer.h"
#include "CANqueue.h"
#include "CANopen.h"

void main_loop(void);
void ms1_loop(void);
void ms10_loop(void);
void ms100_loop(void);

int main(void)
{ 
    /* System Init */
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
    delay_init(168);    //初始化延时函数
    canqueue_init();
    canopen_init();
    set_ms_timer(TIM2, 1);
    set_ms_timer(TIM3, 10);
    set_ms_timer(TIM4, 100);
    CAN1_Mode_Init(CAN_SJW_1tq, CAN_BS2_6tq, CAN_BS1_7tq, 6, CAN_Mode_Normal);    //CAN普通模式初始化,普通模式,波特率500Kbps

    while(1)
    {
        if(time_flags.ms100)ms100_loop();
        if(time_flags.ms10)ms10_loop();
        if(time_flags.ms1)ms1_loop();
        main_loop();
    }
}

void main_loop()
{
    if(!CAN1_queue.is_empty)
    {
        static CanRxMsg msg = {0};
        msg = CAN1_queue.pop();
        canopen_parse_msg(&msg);
    }
}

void ms1_loop()
{
    time_flag_refresh(time_flags.ms1);
}

void ms10_loop()
{
    time_flag_refresh(time_flags.ms10);
    sdo_loop();
    nmt_loop();
}

void ms100_loop()
{
    time_flag_refresh(time_flags.ms100);
    canopen_sync();
}
